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1, INTRODUCTION 

Making an autonomous mobile robot is a great challenge as it requires a lot of unsolved issues. 
Researcher needs to consider various aspects such as the mobile robot structures, control algorithms, 
the surroundings area of observations, and dynamic situations. These factors have made the problem becomes 
difficult to be solved and requires more study considering those aspects. One of the navigation problem is 
known as Simultaneous Localization and Mapping(SLAM) problem that defines a condition where a mobile 
robot attempts to build a map consisting of itself and any observed landmarks concurrently while moving 
throuugh the environment[1]-[3]. SLAM has been a solution to various applications such as exploration, 
underwater navigation and for military purposes. 

Generally there are three approaches in modeling the SLAM problem either by mathematical 
modelling, behavioral model or the probabilistic model. Each of the techniques has their own advantages that 
they are offering to researchers. Most notable performance has been recognized by using the probabilistic as 
there are many research applies Extended Kalman Filter to provide a sufficient information about the 
surrounding. The reason behind this is due to the probabilistics technique provides an efficient information 
with less computational cost and complexity in comparison with the other two methods mentioned above[1]. 

Among issues such as_ uncertainties[4]-[6], dynamic environments[7]-[9], complexity and 
computational cost[10]-[13], this paper attempts to study on reducing the computational cost by using a 
decorrelation technique on the state covariance. It is a problem that many approaches in SLAM are trying to 
solve by utilizing the information obtained from the state covariance. The state covariance is simply a 
representation of uncertainties that the mobile robot holds during its observation. The bigger the state 
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covariance matrix size, more information about its surroundings are gathered[1]. Apart of those information, 
some of the landmarks locations are sometimes not required as only some unique landmarks are sufficient for 
the mobile robot references[2]. However, instead of neglecting landmarks, there is an idea to decorrelate 
some information obtained by the state covariance which in the end can reduce the computational cost as 
addressed by several authors e.g in [4], [11] and [12]. This will be the main perspective to be investigated in 
this paper. 

A number of approaches have been proposed regarding to this matter for example, the covariance 
inflation method [4], and by adding pseudo state covariance to the predicted or updated state covariance [11]. 
The aim of those techniques are same 1.e to make the computation becomes faster and at the same time can 
preserve a good estimation of both mobile robot and the landmarks. However, there are still exist some trade- 
off such that if those methods have been applied, the error can become instantaneously high and result in 
erroneous estimation. Hence, this situation motivates further analysis and investigation in depth about why 
this 1s happening and requires any other possible technique for solution. 

The analysis looks into the behavior of estimation by using the state covariance updates. The matrix 
inversion is this paper priority concerns to be assessed by evaluating the inversion of state covariance through 
a simple analysis of the matrix determinant. One of the possible way to assess the state covariance matrix 1s 
by examining the information matrix. The matrix norm of the information matrix is analyzed to understand 
its relation to the estimation behavior as it contains the magnitude of the information matrix. It was claimed 
that the decorrelation due to covariance inflation must be done properly to guarantee a reliable 
estimation [15]. Therefore, the calculation of state covariance matrix norm or the matrix magnitude can 
provide a picture on how to decorrelate the state covariance matrix without reducing the estimation 
performance. Further details are presented later in this paper. 

The remaining parts of this paper are about the introduction to SLAM and its state covariance which 
will be presented in Section 2. A brief introduction to the Kalman Filter is also presented to provide a basic 
picture of how the system is working. This is followed by the analysis of the information matrix and matrix 
magnitude in Section3. Section 4 assess the proposition made in this paper and finally Section 5 concludes 
the paper. 


2. SLAM AND STATE COVARIANCE 

Two general steps are essential in determining a good localization and mapping by a mobile robot. 
The first step 1s known as process model or kinematic model that defines how the mobile robot moves in an 
environment. It then followed by the measurement or observation model where mobile robot taking relative 
measurements recursively at a period of time. These two steps can be mathematically presented as mentioned 
in the following subsections. 


2.1. Mathematical Formulation 

Consider a state x, € R@t+?™ which consists of mobile robot x,y position and its heading angle with 
a number of n landmarks marked with x,y locations. The process model is represented by the following 
equation. The kinematic model of the system is represented by: 


Xr = f Xp, Ux, @) (1) 


where u, describes the control input that normally consists of the mobile robot velocity and angular 
acceleration. w represents the noise occurred during mobile robot movements. 

As mentioned previously, to achieve SLAM problem, the mobile robot needs to know its 
environment and therefore, the surroundings area must be observed. This is accomplished by using its 
sensors such as sonar sensor, vision sensor or the LRF sensor to measure the relative distance between 
mobile robot and any recognized landmarks during mobile robot observations. The measurement is 
calculated as follow. 


Ze41 = A(X,,V) (2) 


where Z,; describes the measurement matrix which consists of the relative distances and angles between 
mobile robot and landmarks. 


2.2. Kalman Filter and State Covariance 
Kalman Filter is generally build up based on two stages which are the prediction and update. Both of 


these stages are very important to be calculated to make recursive estimation of the mobile robot states and 
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describing the condition of the area to be observed. Prediction stage is referring to the kinematic model of the 
mobile robot to infer the location of the mobile robot based on its movements. On the other hand, the update 
stage innovates the information obtained from the prediction through Kalman gain, to estimates the mobile 
robot and landmarks location at each sampled time. 

The prediction stage holds the following equation. 


Xear = Xe + f Xe Ux) (3) 
where X; is the predicted states with its associated state covariance matrix expressed by: 


Pegi fPp ff FQ) (4) 


For above equation, P, is the predicted state covariance with its associated noise, Q,. 
The information obtained in the prediction stage is then utilized to update the estimated state. 


The updated states %;,, becomes, 

Kiet = Siar + K Gar — Me) Ux) (5) 
where K is the Kalman Gain. 

K = Pry hl (APey gh! + Re) *Ux) (6) 


where R; is the covariance of measurement error produced by the sensor. Together with the state 
updates is the updated state covariance shown as 


Pes = U — Kh)Py Ux) (7) 


Information matrix is one of the available technique to analyze the condition of estimation. 
It is simply the inverse of the state covariance. Both calculation can be done for both prediction stage and 
updated stage. The information matrix at prediction stage yields 


(Vita = Qe? — Qe FCO DT + AQ AY TFT Qe ® (8) 
For the updated stage, it becomes, 
(Oita = Pra = (Pet + AER Th (9) 


This is where the matrix inversion takes place which can consumed a lot of time when doing 
computation especially when the environment to be observed is bigger. Note that when the mobile robot 
observes more landmarks, the state size increases and consequently results in bigger matrix size for the state 
covariance. Note that it is desirable to design a system that has faster computation, as slower computation 
will not makes the SLAM problem becomes practical for real time applications. In matrix calculation 
especially when bigger matrix size 1s concerned, the calculation will be faster if the matrix to be inverted 
only has values on its diagonal elements. If the cross-diagonal elements are zero, then the calculation will be 
simpler. This is the point that motivates this research. To illustrate this, Figures 1, and 2 provide a basic 
picture of the proposal of this research. 





Figure 1. Full state covariance matrix (shaded = Figure 2. Decorrelated state covariance matrix 
area contains real number) (non-shaded area do not contains value) 
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3. INFORMATION MATRIX AND ITS MAGNITUDE 

This section investigates how the information matrix 1.e the inverse of state covariance matrix 
behaves if some parts or elements in the matrix are modified to ease and makes faster computations. To begin 
with consider the following idea of a monobot system observing a landmark. Assume that there is a 
semidefinite matrix, P and P > 0. Note also that, all elements in P is bigger than 0. 


af 10 


The inverse of P can be shown as follow. 


-,_fa B 
a =|) f (11) 


The inverse of matrix P is calculated based on normal matrix inverstion formula presented as follow. 


Ce B a d ae (12) 


y oO ad-—bcl—-c @q 





The calculation will be much more simpler whenever the element of b and c in P are equal to zero. 
In this case, those elements are called the cross-correlation of matrix P. When this is happening, then the 
inversion becomes 


cent | 


In SLAM, the updated covariance cannot be decorrelate without sufficient consideration on how the 
elements are connected to each other. If those correlations are removed without proper modifications, then 
the estimation results exhibit erroneous results such that, the uncertainties suddenly becomes higher. 
By neglecting those correlations also means that, the mobile robot states do not has any confident in where it 
is located and becomes lost. Looking at different perspective, matrix P in equation above can also be 
represented by the following equation where some pseudo values are being added to it. 


(13) 


So air 
alrF © 


al — 1 - 0 
ere ae ae ll F : “ 
d 

These two equations yield the same result. Such condition has been illustrated in the preceding 
research where they called it as unstable partially observable SLAM [5]. This condition cannot guarantee a 
good estimation if it is happening during estimation process. 

Now the remaining matter to understand is to ensure that the state covariance features the same 
information even though in matrix decorrelation. One of the answer is by analyzing the matrix magnitude or 
matrix norm. Matrix norm demonstrates the matrix magnitude or in SLAM case, the information that mobile 
robot possess during its observations. If the information is modified from having a large information to 
smaller information, then the mobile robot is expected to has less information. Hence, the mobile robot holds 
less confidence about its whereabout. 

The analysis will focus only on euclidean norm as it demonstrates similarly to the pythagorean 
length on describing how the mobile robot moves. Again, examine matrix P defined in 10. 
Remark that to guarantee a positive semidefinite matrix, the element of a, d > 0. In addition, in SLAM, 
the state covariance matrix is symmetric. Hence its inverse holds the same properties i.e symmetric. 
By calculating the euclidean norm of the information matrix, 


norm(Q) = [4] (d*+b*+c*+a’*)>0 (15) 


Note that the above equation yields a real number at all time. When element b, and c are being 
decorrelated, then 





norm(O,) = {[*]° + [4] > 0 (16) 
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Comparing between these two conditions, clearly, 

norm(Q.) > norm(Q,.) > 0 (17) 
This criteria defines that the norms can be related directly to information obtained by the mobile robot. 


One more important thing discovered 1s that, the inverse of the information matrix can not exhibit infinite 
such that, 





ae aac a (18) 


If the inverse of information matrix becomes infinite, then it is impossible to estimate the information 
obtained by the mobile robot. These two behaviors lead to below proposition and lemma. 

The analysis now moved to the case of using Covariance Inflation method. Based on the theoretical 
descriptions, the updated state covariance becomes smaller than the previous estimation results[4]. Utilizing 
this information, the following are proposed. 

Proposition : Mobile robot has less information about its surrounding if there is any decorrelation of its non- 
diagonal elements. 

Proof : The proof can be easily obtained based on eqs.(10)-(12) for each time sequence and therefore 
omitted. 

Lemma | : Assume that an information matrix, 0 € IR"*" is a semidefinite matrix and holds the following 
conditions. 


Os =A, = 0 (19) 


Proof — : The proof can similarly be obtained from eqs.(10)-(18) and therefore omitted. 
The information matrix holds a matrix norm higher than the decorrelated information matrix norm if the 
following are satisfied. 

a) The inverse of information matrix do not exhibit infinite during mobile robot observations. 

b) At least some of the non-diagonal elements of the state covariance matrix are non-zero. 


3.1. Numerical Example 

To assess the above proposed Lemma 1, the following examples are presented. The examples are 
divided into three different cases consisting of the system that do not posses any decorrelation, the one that 
has partial decorrelation and finally the one that has full decorrelation of the state covariance. 
a) Accase of the updated state covariance when the mobile robot is decorrelated to some of the landmarks. 


Suppose that 
1 0.9 0 
P= os 1 -04| (20) 
0 —0.2 3 


Calculating the norm and determinant of the above matrix gives, 
norm(P) = 3.0246, determinant (P) = 0.53 (21) 


b) A case of the updated state covariance when the mobile robot is decorrelated to all landmarks. As 
shown in (19) is then being decorrelated about its non-diagonal element that then yields, 





1 0 0 
Py =|0 1 ] (22) 
0 0 3 
If this happen, then the norm and determinant becomes, 
norm(P,) = 3,determinant (P,) = 3 (23) 


Clearly by observing the matrix norm, the second case yields smaller norm. This demonstrates a 
case when mobile robot uncertainties becomes higher as there is less information about its surrounding. 
This is also means that the robot do not has any reference landmarks located near to it. Smaller matrix norm 
leads to less information obtained by the mobile robot and could ended in erroneous results. 
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c) Adding pseudo state covariance to the updated state covariance case resulting in P,. 
If the diagonal elements of the updated state covariance of (21) is added by a pseudo covariance, 
then the matrix norm becomes bigger than (22) such that 
P»>P>P, 
This result 1s also explaining what has happened to the results presented by literatures [4], [11] 
as adding a pseudo state covariance can lead to erroneous estimation. 


4. SIMULATION ANALYSIS AND DISCUSSION 

To evaluate the performance of decorrelation for better view of what is happening to the estimation, 
simulation on certain conditions are presented. The simulation parameters are described in Table 1 for 
references. The simulation focuses on EKF for both gaussian noise and non-gaussian noise. The reason why 
to include these aspects is to observe the differences of EKF performance when decorrelation of state 
covariances are considered. 

For the gaussian noise case, the following figures are obtained. The decorrelation may used either 
the one presented in [4] or [11]. In this research, the method presented in [4] 1s referred. Figure 3 described 
the performance of normal EKF in comparison to the EKF with partial decorrelation. The decorrelation did 
not makes the estimation faulty and preserved good results. The MSE assessment in Figure 4 shows that the 
EKF with partial decorrelation has better performance compared to the normal EKF. This is the case when 3 
landmarks are being decorrelated. Even if the based on the proposed conditions stated in previous section 
shows that fewer information is available when decorrelation is in place, the mobile robot still preserved a 
good estimation results. One of the possible reason why this is happening can be expected due to the EKF 
inconsistency reported in various numbers of research. The estimation becomes too optimistic and lead to the 
illustrated results. This conditions lead to further investigation that will be organized in future. 


Table 1. Simulation Parameters 
Simulated values 


ea 


Parameters 
Initial State 


; P, 0 
Covariance EK | 


Gaussian noise for both 
process noise, Q and 


1x 1077 0 
measurement noise, R | | 


0 1x 107? 
orl 
0 R 
Uniform noise for both Omin = —0.002 
process noise, Q and 0... = 0.001 
measurement noise, R Rn. = 0 15 
Q 0 min _ . 
| 0 I Rmax = 0.3 
Simulation time 1000[s] 
Sampling time 0.1 
Landmark number 13 


250 r 4 





500 - 


250 » 





+ | ——— — ——— 
+ nen) | —-—- x coordinate | —-—-x coordinate (Landmark 1) 
450} | === y coordinate | a —-—-y coordinate (Landmark 1) 
200 by 774 | —~—~ theta i | «=x coordinate (Landmark 2) 
| 400 b iy 200 | —-—~ y coordinate (Landmark 2) 
| . i | 
| ! | e i 
1so ? Say H 7 s : 
rp ts r : gE S50 He | 2 és 
5 oy i i 
— 7 ~ i S i 
2 2 300F ei 2 150F i* . 
= 100} = i 4 Ps eee i 
aor = i if = i‘ i” 
~ "76 s — s * v2 
5 A 250) ——a a ry’ 
S r= 1 ¢ = 4 x o 
3 S0+- 4 =) = 5 i . i a ee | ' " 5 
> ‘= 200+} Hp i! ' = 100+ i | 
= i s 5 i S s ~ 
5 j i Py = = 1 
% } ‘ ‘, 8 = 
oF 2°; WL AT aE 
’ 
+ 2A af iit i i ' . Ht rez | . 
100 igi tits iF iif 50--—_ i een] 
sot 8 Pa aiE A Rin itdl posed 
-so} ‘ ot TEA AW LA AER L.. | 
+ St} i jt ny el iteqiik « Mi fi" smn perenne 
L_. , , | pRUL AY pe tp iy Ute | pws foo 
_9 15 “10 7, ha z ae +1 7 is} ‘ ; * ie! | 
200 150 100 50 0 50 100 150 200 jee a ee eh ee .. a 


X coordinate [cm] 


Figure 3. Estimation comparison between normal 
EKF with gaussian noise (black) and EKF with 
partial decorrelation (blue). The true path is mark 
as green 
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Figure 4. MSE results in gaussian noise between 
normal EKF(dash line) and EKF with partial 
decorrelation(solid line). 
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Figure 5-7 are the case when only 3 landmarks are being decorrelated for non-gaussian noise case. 
Interestingly EKF with partial decorrelation still guarantees a good performance compared to the 


normal EKF. 


250 


200 


Ww 


Y coordinate [cm] 





-50) 








200 -150 -100 -50 0) 50 


X coordinate [cm] 


100 150 200 


Figure 5. Estimation comparison between normal 
EKF with uniform noise (black) and EKF with 
partial decorrelation (blue). The true path is mark 
as green 
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Figure 6. MSE results in uniform noise between 
normal EKF(dash line) and EKF with partial 
decorrelation(solid line) 
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Figure 7. Estimation comparison between normal EKF with uniform noise (black) and EKF with full 
decorrelation (blue). The true path is mark as green. 


The following Figures 7-8 are the results of decorrelating almost all landmarks. Surprisingly, 
even though almost all landmarks have been decorrelated to the mobile robot state covariance, the estimation 
can still preserved good results. This is due to the addition of the pseudo state covariance that able to 
compensate the decorrelation of the landmarks. Note that, the value of pseudo state covariance are selected 
based on certain readings of the non=diagonal state elements. Different to what previous research has 
suggested, the value of pseudo state covariance to add at each updated state covariance is changed as 
proposed earlier in (14). If the value of the pseudo state covariance is not correct or too big, then the state 
estimation become erroneous as shown in Figure 9. This is also called as unstable partially observable 
SLAM, where all non-diagonal elements in the updated state covariance are eliminated without considering 


their relations. 
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Figure 8. MSE results in uniform noise between 


5. 


normal EKF(dash line) and EKF with full Figure 9. Estimation becomes erroneous when all 
decorrelation(solid line) landmarks are 
CONCLUSION 


This paper introduced the analysis of partially observability in SLAM problem by considering the 


effect of matrix inversion and decorrelation to the whole system. It was suggested that the information matrix 
can define the estimation performance if the information matrix is not suddenly become infinite and some of 
the elements in the state covariance is still holding a non-zero element. In addition, the results are consistent 
and agrees with what the preceding authors has claimed. 
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